#include <ros/ros.h>
#include "kinematic.h"
#include <ubt_core_msgs/JointCommand.h>
#include "webots_api/SceneSelection.h"

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "GraspCup_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");
    
    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "GraspCup";
    scene_selection.request.nav = false;
    scene_selection.request.vision = false;
    
    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    ros::Publisher left_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftHand/controller", 10);
    ubt_core_msgs::JointCommand left_finger_data;
    left_finger_data.command.resize(10);
    left_finger_data.mode = 5;

    Eigen::VectorXd joints(7);
    joints << 0,0,0,0,0,0,0;
    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0.0;
    while(ros::ok())
    {
        time_count += 0.004;
        if(time_count < 1)
        {
            orientation << 0,0,0;
            position << 400+160*sin(time_count*M_PI/2), 0, 400 - 200*sin(time_count*M_PI/2);
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 2)
        {
            position << 560, 250*(time_count-1), 200;
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 2.5)
        {
            position << 560, 250-80*(time_count-2), 200-60*(time_count-2);
            if(leftArmInverse(position, orientation, joints, false) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = 0.38;
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 2.8)
        {
            orientation << 0, M_PI*4/9,0;
            position << 540, 210, 170;
            if(leftArmInverse(position, orientation, joints, true) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 3.3)
        {
            orientation << 0, M_PI*4/9,0;
            position << 540-90*(time_count-2.8), 210+40*(time_count-2.8), 170+160*(time_count-2.8);
            if(leftArmInverse(position, orientation, joints, true) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 3.5)
        {
            orientation << 0, M_PI*4/9,0;
            position << 495-75*(time_count-3.3), 230+250*(time_count-3.3), 250;
            if(leftArmInverse(position, orientation, joints, true) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 3.7)
        {
            orientation << 0, M_PI*4/9,0;
            position << 480-75*(time_count-3.5), 280+50*(time_count-3.5), 250;
            if(leftArmInverse(position, orientation, joints, true) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        else if(time_count < 3.8)
        {
            float angle = 0.55;
            left_finger_data.command[0] = 0.2;
            left_finger_data.command[1] = 0.2;
            left_finger_data.command[2] = angle;
            left_finger_data.command[3] = angle;
            left_finger_data.command[4] = angle;
            left_finger_data.command[5] = angle;
            left_finger_data.command[6] = angle;
            left_finger_data.command[7] = angle;
            left_finger_data.command[8] = angle;
            left_finger_data.command[9] = angle;
            left_finger_pub.publish(left_finger_data);
        }
        else if(time_count < 4)
        {
            orientation << 0, M_PI*4/9,0;
            position << 465, 290, 250-250*(time_count-3.8);
            if(leftArmInverse(position, orientation, joints, true) == true)
            {
                left_arm_data.command[0] = joints(0);
                left_arm_data.command[1] = joints(1);
                left_arm_data.command[2] = joints(2);
                left_arm_data.command[3] = joints(3);
                left_arm_data.command[4] = joints(4);
                left_arm_data.command[5] = joints(5);
                left_arm_data.command[6] = joints(6);
                left_arm_pub.publish(left_arm_data);
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}